/**
*  \file bdi.c
*
*  \brief
*
*  \author Dajin Li  <dajin.li@linde-.de>
*
*
*/

/*===============================================[ private includes  ]================================================*/
#include "stm32f10x.h"
#include "global.h"
#include "analoginput.h"
#include "var_tbl.h"
#include "icons.h"
#include "menu.h"

#include "bdi.h"

/*===============================================[ private defines   ]================================================*/
#define         ChIsZeroThreshold      (5)  // Below this value, detect erroneous runs of zeros
#define         ChIsHighThreshold     (10)  // Above this value was not a zero
#define         Ch100pc            (28800)  // Charge at 100% is assumed to represent 8hrs [in seconds]
#define         ButterRestartGradient (10)  // Butterworth restarting condition gradient (a negative reduction)

// ts = 10s, w0 = 0.0005 rad.s-1, factor = 1 + sqrt(8)/(Ts*w0) + 4/pow(Ts*w0,2) = 160567
#define         CoeffScale1      (1000000)  // used to convert floating point to integer maths
#define         Coeff_uk               (6)  // Butterworth coefficient for u(k)
#define         Coeff_ukm1            (12)  // Butterworth coefficient for u(k-1)
#define         Coeff_ukm2             (6)  // Butterworth coefficient for u(k-2)
#define         CoeffScale2          (100)  // used to convert floating point to integer maths
#define         Coeff_ykm1           (199)  // Butterworth coefficient for y(k-1)
#define         Coeff_ykm2           (-99)  // Butterworth coefficient for y(k-2)

/*===============================================[ private datatypes ]================================================*/

/*===============================================[ private variables ]================================================*/



/*===============================================[ private functions ]================================================*/
static void BDI_Display(void);
static void BDI_Model(void);
void Push_BatSoc(void);
u32 Pop_BatSoc(void);
/*================================================[ inline functions ]================================================*/
/*===============================================[ public variables ]================================================*/


//----- Global BDI Variables -----
s8 BDI_BattCharge;       // Battery Charge - State Of Charge reading 0 - 100%
s8 BDI_BattCon;          // Battery Condition -  0 - 100%
u16 BDI_VBatt ;          // Coarse Battery Reading allow negative volts scale
s32 Vin;                 // Scaled voltage in reading used in the calculation
u8 BDI_Error;            // error flag - 0 = no error
u8 BDI_LowBattFlag;      // Set when BattCharge < 2%
u8 BDI_BattCutFlag;      // Set when BattCharge < 1%
u8 BDI_FC_Flag;          // Fine/Coarse flag - Set when Fine out of range
u8 BDI_LowBattConFlag;   // Set when BattCon < 1.94V
s32 BattSOC;             // State-Of-Charge modelled voltage
s32 BattCon;             // Battery Condition modelled voltage
s32 Batt_In;             // Rescaled Battery Input voltage for model
s32 last_Batt_In;        // stored for use in BattCon recovery
s32 Vempty ;
u8  BDI_run;             // flag to start BDI after initalisation
//--------------------------------
s32 Volts_Stable_cnt;
s32 bdicnt;

//-------- Qmon Stuff --------------
s32  Qmax;               // current max voltage seen in the last 10s
s32  Qmin;               // current min voltage seen in the last 10s
s32  Qmax_next;          // next max voltage seen in the last 10s
s32  Qmin_next;          // next min voltage seen in the last 10s

s32  Qmax_cnt;           // time stamp for current Qmax
s32  Qmin_cnt;           // time stamp for current Qmin
s32  Qmax_next_cnt;      // time stamp for next Qmax
s32  Qmin_next_cnt;      // time stamp for next Qmin
s32  Qmon_cnt;           // Qmon timer
//----------------------------------
u8   xEXTERNAL_BATTERY ;     // previous state of ExtBatt input

//---- Hrs Remaining calculation ----
s32 BDI_HrsRem;             // global for holding Estimation of hours remaining as hours x10 i.e. 86=8.6 hours
s32 xBattCharge;            // local  previous battery charge value
s32 cntBattHrSamples;       // count the number of samples so we know when to start using the values - ignore initial one(s) cos they may be inaccurate
s32 BattTimer;              // timer for timing between % decreases of capacity
u8 BDI_HrsRem_init;         // initialisation flag

u8 BDI_PEDtest;             // test bit to hook into BDI for testing
s16 BDI_vbatt_cal[3];       // single calibration point
s32 LIDtest[8];             // data buffers for debug/test data outputs on CAN
s32 Qmon ;  // the cell activity in 0.1mV/bit (i.e. 5000=0.5V/cell). This is the difference between the min & max recorded values. 0 = stable voltage - not in use.
s32 pre_soc;            // last time adc
/*================================================[ public functions ]================================================*/
/**
* @brief	Init bdi
* @param	none
* @note     none
* @retval   none
*/
void bdi_init(void )
{  
    BDI_HrsRem_init = 0; // PT
    BDI_HrsRem = -1 ;

    BDI_run = 0;
    BDI_PEDtest = 0;

    BDI_BattCharge = -1;
    BDI_BattCon = -1;

    BDI_Error = 0;
    BDI_LowBattFlag = 0;    // assume good volts at startup until BDI_display says otherwise
    BDI_BattCutFlag = 0;    // assume good volts at startup until BDI_display says otherwise
    BDI_LowBattConFlag = 0; // assume low batt condition at startup

    BDI_FC_Flag = 1;        // assume coarse reading
    BDI_VBatt = 0 ;         //-1 NG 27.04.04 for Lint;

    BattSOC = 0;
    BattCon = 0;
    Batt_In = 0;
    last_Batt_In = 0;

    Volts_Stable_cnt = 0;

    bdicnt = 0;

    //----- Initalise Quiescent Monitoring -----
    Qmax = 0;
    Qmin = 25000;
    Qmax_next = 0;
    Qmin_next = 0;

    Qmax_cnt = 0;
    Qmin_cnt = 0;
    Qmax_next_cnt = 0;
    Qmin_next_cnt = 0;
    Qmon_cnt = 0;
    
    BDI_cutvpc = 173; 
//    pre_soc = RTC_ReadBackupRegister(RTC_BKP_DR11);    
    pre_soc = Pop_BatSoc();
} 

/**
* @brief	Backup register store BATT SOC
* @param	nonr
* @note     none
* @retval   none
*/
void Push_BatSoc(void)
{
    BKP_WriteBackupRegister(BKP_DR1, BattSOC);
    BKP_WriteBackupRegister(BKP_DR2, BattSOC >> 16);
}

/**
* @brief	Backup register read back BATT SOC
* @param	nonr
* @note     none
* @retval   none
*/
u32 Pop_BatSoc(void)
{
    u32 batt_temp1, batt_temp2;
    batt_temp1 = BKP_ReadBackupRegister(BKP_DR1);
    batt_temp2 = BKP_ReadBackupRegister(BKP_DR2);
    batt_temp2 = ((batt_temp2 << 16) & 0xffff0000) | batt_temp1;
    return batt_temp2;
}

/**
* @brief	This task update the fuel capacity
* @param	void const *argument
* @note     none
* @retval   none
*/
void BDI_task(void const *argument) 
{
    // Initialization 
    task_status[UPDT_BDI_ID] = ALIVE;
    bdi_init();             //initialize bdi model
    // Tasks must run in an infinite loop:
    while (TRUE)
    {
        bdi_task();
        task_status[UPDT_BDI_ID] = ALIVE;
        osDelay(UPDT_BDI_ITV);
	}  // end while infinite loop
}  // end task_updt_BDI()
/**
* @brief	This task calculate the vin
* @param	void const *argument
* @note     none
* @retval   none
*/
void BDI_Meas_task(void const *argument) 
{
    // Initialization 
    task_status[UPDT_BDI_ID] = ALIVE;
    bdi_init();             //initialize bdi model
    // Tasks must run in an infinite loop:
    while (TRUE)
    {
        BDI_VBatt  = (u32)ADC_VBATT *2400 / BDI_vbatt_cal[0]; //BDI_vbatt_cal[0];         // batt volts input scale and single point cal adjust
        Vin =((s32)BDI_VBatt*1000)/120;                           // Vin scaled to give volts-per-cell (20833=2.0833V) for a 48V battery
        task_status[UPDT_BDI_ID] = ALIVE;
        osDelay(UPDT_BDIMEAS_ITV);
	}  // end while infinite loop
}  // end task_updt_BDI()
/**
* @brief	This task update the fuel capacity
* @param	void const *argument
* @note     none
* @retval   none
*/
void bdi_task(void) 
{
//    BDI_VBatt  = (u32)ADC_VBATT *2400 / 3713; //BDI_vbatt_cal[0];         // batt volts input scale and single point cal adjust
//    Vin =((s32)BDI_VBatt*1000)/120;                           // Vin scaled to give volts-per-cell (20833=2.0833V) for a 48V battery
    Vempty = (s32)BDI_cutvpc  * 1000000L;  // initialise empty value (1.94V/Cell etc) - done before calc every time in case it's changed
    BDI_Model();
    BDI_Display();     // BDI: Displaying on the Console
}  // end task_updt_BDI()	



/*===============================================[ private functions ]================================================*/



/*****************************************************************************
        Function:  BDI_Display
     Description:  converts the batt volts to % charge and displays it
******************************************************************************/
static void BDI_Display(void)
{
    s32 tmp_l;

    if(BDI_run && !BDI_PEDtest)
    {
        tmp_l = 0;

        // first predict actual state of charge (SOC)
        tmp_l = BattSOC - Vempty;

        if(tmp_l > -1)
        {
            tmp_l /= ((Vfull - Vempty) / 100);
            if (tmp_l > 100) 
            {
                tmp_l=100 ;  // catch overcharge to stop wrapping
            }
            BDI_BattCharge = tmp_l; //BattCharge is now a %
        }
        else
        {
            BDI_BattCharge = 0;
        }

        // now the current battery loading (CBL)
        tmp_l = BattCon - Vempty;

        if(tmp_l > -1 )
        {
            tmp_l /= ((Vfull - Vempty)/100);
            if(tmp_l > 100)
            {
                tmp_l = 100 ;  // catch overcharge to stop wrapping
            }
            BDI_BattCon = tmp_l;
        }
        else
        {
            BDI_BattCon = 0;
        }

        // sort out segment so CBL is never greater than SOC

        if(((BDI_BattCon) / 10)>((BDI_BattCharge) / 10))
        {
            BDI_BattCon = BDI_BattCharge;
        }


        // Battery discharge warnings
        if(BDI_BattCharge < 2)
        {
            BDI_LowBattFlag = 1;
        }
        else 
        {
            BDI_LowBattFlag = 0;
        }
        if(BDI_BattCharge < 1)
        {
            BDI_BattCutFlag = 1;
        }
        else
        {
            BDI_BattCutFlag = 0;
        }
    }// end if(BDI_go)
//   Dg_BattCharge = (int)(BDI_BattCharge & 0x7f) ;
}//BDI_Display end

/*****************************************************************************
        Function:  BDI_Model
     Description:  This is the clever bit
******************************************************************************/
static void BDI_Model(void)
{
    s32 kk0;
    s32 kk1;
    s32 a;
    s32 b;
    s32 d;

    // Qmon moved to here 01.07.04 ROJ
    if(Vin > Qmax)
    {
        Qmax = Vin;
        Qmax_cnt = Qmon_cnt;
    }
    else
    {
        if(Vin > Qmax_next)
        {
            Qmax_next = Vin;
            Qmax_next_cnt = Qmon_cnt;
        }
    }

    if(Vin < Qmin)
    {
        Qmin = Vin;
        Qmin_cnt = Qmon_cnt;
    }
    else
    {
        if(Vin < Qmin_next)
        {
            Qmin_next = Vin;
            Qmin_next_cnt = Qmon_cnt;
        }
    }

    if(Qmon_cnt > QTIME) 
    {
        Qmon_cnt = 0;
    }
    else  
    {
        Qmon_cnt++;
    }

    if(Qmon_cnt == Qmax_cnt)
    {
        Qmax = Qmax_next;
        Qmax_cnt = Qmon_cnt;
        Qmax_next_cnt = Qmax_next = 0;
    }

    if(Qmon_cnt == Qmin_cnt)
    {
        Qmin = Qmin_next;
        Qmin_cnt = Qmon_cnt;
        Qmin_next_cnt = Qmin_next = 25000;
    }

    if(bdicnt < 2)    // Initial value calculation from battery volts
    {
        BattSOC = (s32)Vin*10000;
        if((ABS(BattSOC - pre_soc) < 680000) && (BattSOC < VfullRst))
        {
            BattSOC = pre_soc;
        }
        BattCon = BattSOC;
        bdicnt++; // should be good data so display it
    }
    else
    {
        BDI_run = 1;                 // start BDI shortly after powerup to overcome random data at startup

        last_Batt_In = Batt_In;      // store for recovery correction for BattCon
 
        Batt_In = (s32)Vin * 10000;   // Rescale to allow small inc/dec for model

        //--------------- SOC ---------------
        a = 0;  // if batt_in  > battSoc, a should be 0, otherwise if BattSOC > 2.14, a = 2
        b = 0;  // if b is in normal range, this is 0
        d = 0;

        if(Batt_In - BattSOC >=0)
        {
            kk1 = k1;
            kk0 = k0; // 25000000=0.25V
        }
        else
        {
            if(BattSOC > Vfull) //2.14V
            {
                a += 2; // Vin < SOC && SOC > 2.14v per cell a += k3, decrese faster
            }

            kk1 = k1/k2;
            kk0 = 0;
        }


        //----- Quiescent Voltage monitor & error correction -------
        // This is used to correct errors in the SOC model

        if(BattSOC > Batt_In)
        {
            if((Qmin < 25000) && (Qmax > 0))// only after first value 2.5V
            {
                if((Qmin + 100) >= Qmax)// (Qmax - Qmin) < 0.01V
                {
                    a += 4;
                    d = 8;//set BattCon condition here
                }
            }
        }
        
        if (Vin > 24000) // 2.4V  2.4*24 = 57.6V
        {
            b += 5; // Vin > Vgas a = a + k7     ... Gassing
        }
        else if(Vin > 21800)//2.18V BAT 2.18*24=52.32
        {
            b += 1; // Vin > Vhi  a = a + k6     ... Full Charge
        }
        else if(Vin < 15500) // 1.55V 1.55*24=37.2
        {
            b += 1;            // internal resistance factor
        }
        else  
        {
            b =  0;
        }

        //---------------------------------------
        // full charge  need to (218)
        BattSOC += ((Batt_In - BattSOC + kk0) * (1 + a))/((1 + b) * (kk1));

        //---------------------------------------------------------

        // ---- BattCon ----
        if(Batt_In - BattCon >=0)
        {
            kk1 = k1;
            kk0 = k0;
        }
        else
        {
            kk1 = 13333;
            kk0 = 0;
        }

        a = 0;

        if (Vin < 12000)
        {
            a += 9;
        }
        if (Vin < 13000)
        {
            a += 5;
        }
        if (Vin < 14000)
        {
            a += 3;
        }
        if (Vin < 15000)
        {
            a += 1;
        }

        // BattCon = BattCon + ((((Batt_In - BattCon)*(1+c))+(kk0*k8)+d)/kk1)
        BattCon += (((Batt_In - BattCon) * (1 + a) + (kk0 * 2) + d)/kk1);
 
        if(BattCon > BattSOC)  
        {
            BattCon=BattSOC;   // BattCon can never be greater than BattSOC if BattSOC > 1.93 V per cell!!!
        }
        if(BattCon < 193000000)
        {
            BattCon=193000000; // impose 1.93 volt limit on BattCon
        }
//        RTC_WriteBackupRegister(RTC_BKP_DR11, BattSOC);
        Push_BatSoc();
    }
}// end BDI_Model
//---------------------------------------------------------------------------



/***********************************************************************************************************************
*                             Copyright 2016 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/


